using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{

    public struct TwoBody1DOFJacobians
    {
        public Vector3Wide LinearA;
        public Vector3Wide AngularA;
        public Vector3Wide LinearB;
        public Vector3Wide AngularB;
    }



    public struct Projection2Body1DOF
    {
        // 我们不是使用JT从世界空间投影到约束空间*速度*,而是预先计算JT*有效质量
        // 直接从世界空间速度到约束空间冲量。
        public Vector3Wide WSVtoCSILinearA;
        public Vector3Wide WSVtoCSIAngularA;
        public Vector3Wide WSVtoCSILinearB;
        public Vector3Wide WSVtoCSIAngularB;

        // 由于我们直接从世界空间速度跳到约束空间冲量,速度偏差也需要预先计算成冲量偏移量。
        public Vector<float> BiasImpulse;
        // 再一次,CFM变成了CFM*EffectiveBulk-由于CFM的派生而大量抵消。(请参阅前置说明。)
        public Vector<float> SoftnessImpulseScale;

        // 它还需要从约束空间投影到世界空间。
        // 我们把它和惯性/质量乘数捆绑在一起,所以不是把约束脉冲带到世界脉冲,然后带到世界速度的变化,
        // 我们只是直接从约束冲动到世界速度的变化。
        // 对于自由度数较低的约束,使用此格式还可以节省一些内存带宽-
        // 与此实现的12相比,2体约束的逆惯性张量和逆质量成本为20浮动。
        // (请注意,即使在我们使用主体惯性的实现中,我们仍然应该约束地本地缓存它,以避免大的聚集。)
        public Vector3Wide CSIToWSVLinearA;
        public Vector3Wide CSIToWSVAngularA;
        public Vector3Wide CSIToWSVLinearB;
        public Vector3Wide CSIToWSVAngularB;
    }

    public static class Inequality2Body1DOF
    {

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector<float> maximumRecoveryVelocity,
            ref Vector<float> positionError, float dt, float inverseDt, out Projection2Body1DOF projection)
        {
            // 未软化有效质量=(J*M^-1*JT)^-1
            // 其中J是约束DOF x Bodycount*6大小的矩阵,JT是它的转置矩阵,对于两体M^-1是：
            // [反转体量A,0,0,0]
            // [0,InverseInertiaA,0,0]
            // [0,0,InverseMassB,0]
            // [0,0,0,inverseInertiaB]
            // J的条目符合此约定,按顺序包含每个实体的线性和角度分量,因此对于两个实体1自由度约束J将如下所示：
            // [线性A 1x3,角度A 1x3,线性B 1x3,角度B 1x3]
            // 请注意,按照惯例,它是行向量。将速度从世界空间转换到约束空间时,假设速度矢量被组织为
            // 行向量匹配到雅可比矩阵(即[线性A 1x3,角度A 1x3,线性B 1x3,角度B 1x3]),因此对于二体2自由度约束,
            // worldVelocity*Jt将是[worldVelocity：1x12]*[Jt：12x2],从而产生1x2约束空间速度行向量。
            // 类似地,当在上面的示例中从约束空间脉冲转到世界空间脉冲时,我们将执行[CSI：1x2]*[J：2x12]来获得1x12世界脉冲行向量。

            // 请注意,引擎使用所有速度和位置的行向量,依此类推。旋转张量和惯性张量被构造用于预乘。
            // 换句话说,与空间中的许多演示不同,我们使用v*Jt和csi*J而不是J*v和jt*csi。
            // 没有什么有意义的区别-这两个约定只是彼此的换位。

            // (如果您想了解这些内容是如何工作的,请阅读与约束相关的演示文稿：http://box2d.org/downloads/
            // 要注意惯例的不同。例如,您将看到J*v而不是v*Jt。不过,从根本上说,一切都是一样的。)

            // 由于质量矩阵的挡路结构,我们可以分别处理每个分量,然后将结果求和。
            // 对于此单自由度约束,结果是一个简单的标量。
            // 请注意,我们存储J*M^-1的中间结果,以便在从约束空间脉冲投影到世界速度变化时使用。
            // 如果我们不存储这些中间值,我们可以只缩放Jacobians.LinearA本身的点积,以节省4次乘法。
            Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA);
            Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB);
            Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA);
            Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB);

            // 角度分量稍微复杂一些;(J*i^-1)*JT是显式计算的。
            Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA);
            Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB);
            Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA);
            Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB);

            // 现在让我们离题吧！
            // 沿对角线应用柔和度(对于1自由度约束,该对角线是唯一的元素)。
            // 有关更多信息,请查看ODE参考：http://ode.org/ode-latest-userguide.html#sec_3_8_0
            // 有关更多详细信息,请参阅Erin Catto的软约束演示文稿：http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf)

            // 不过,您可以在这里使用一些非常有趣的技巧。
            // 我们的核心调谐变量是阻尼比和固有频率。
            // 我们的运行时使用的变量是柔软度和误差减少反馈尺度。
            // (对于下面的内容,我将使用ODE术语CFM和ERP、约束力混合和错误减少参数。)
            // 因此,首先,我们需要从阻尼比和固有频率到刚度和阻尼弹簧常数。
            // 从那里,我们将转到CFM/ERP。
            // 然后,我们将创建软化有效质量矩阵的表达式(即考虑CFM项的表达式),
            // 求解迭代中约束力混合项的表达式。
            // 最后,计算ERP。
            // (然后是一些小把戏。)

            // 1)将阻尼比和固有频率转换为刚度和阻尼常数。
            // 原始表达式为：
            // 刚度=有效质量*自然频率^2
            // 阻尼=有效质量*2*阻尼比*自然频率
            // 使用约束的有效质量,而不是使用任何单个对象作为此转换中涉及的"质量"术语的参考。
            // 换句话说,我们正在动态挑选实现当前约束配置所需行为所需的弹簧常量。
            // (有关这方面的更多详细信息,请参阅上面Erin Catto的演示文稿。)

            // (请注意,这不同于BEPUPhysical v1。在那里,用户配置刚度和阻尼常数。这很管用,但是人们经常会搞不清楚
            // 为什么约束在改变质量时表现不同。通常情况下,它表现为某人相对于默认值创建了一个质量极高的对象
            // 刚度/阻尼,他们会在论坛上发帖,想知道为什么约束这么软。基本上,默认值是另一个容易出错的偷偷摸摸的调整因素。
            // 因为阻尼比和固有频率定义了与质量无关的行为,所以这个问题就消失了--而且它使一些其他有趣的事情发生了……)

            // 2)将刚度和阻尼常数转换为CFM和ERP。
            // CFm=(刚度*dt+阻尼)^-1
            // ERP=(刚度*dt)*(刚度*dt+阻尼)^-1
            // 或者,换个说法：
            // ERP=(刚度*DT)*CFM

            // 3)利用CFM和ERP建立软化的有效质量矩阵和用于求解迭代的力混合项。
            // 从我们不会派生的基本定义开始,即速度约束本身(在此声明为相等约束)：
            // 这意味着"投射到约束空间的世界空间速度应等于速度偏移项和约束力混合项之和"。
            // (稍后将计算速度偏差项-它是由误差减少参数ERP缩放的位置误差。位置误差用于创建速度马达目标。)
            // 我们在这里放弃了顺序脉冲的实现,所以我们不使用术语"累积脉冲",而使用"lambda"。
            // (这恰好与涵盖相同主题的ODE文档一致)。λ是满足约束的冲量。
            // wsv*jt=偏置λ*cfm/dt
            // 这可以表述为：
            // CurrentVelocity=目标速度
            // 或者：
            // goalVelocityChange=targetVelocity-Current Velocity
            // λ=goalVelocityChange*有效质量
            // λ=(目标速度-当前速度)*有效质量
            // λ=(偏移-λ*cfm/dt-当前速度)*有效质量
            // 为lambda解算：
            // λ=(偏移电流速度)*有效质量-λ*cfm/dt*有效质量
            // λ+λ*cfm/dt*有效质量=(偏移电流速度)*有效质量
            // (λ+λ*cfm/dt*有效质量)*有效质量^-1=偏置电流速度
            // λ*有效质量^-1+λ*cfm/dt=偏置电流速度
            // λ*(有效质量^-1+cfm/dt)=偏置电流速度
            // λ=(偏置电流速度)*(有效质量^-1+cfm/dt)^-1
            // λ=(偏移-wsv*jt)*(有效质量^-1+cfm/dt)^-1
            // 换句话说,我们使用矩阵(EfficitiveMass^-1+cfm/dt)^-1将速度变化(bias-wsv*jt)转换为满足约束的冲量lambda。
            // 这个矩阵就是软化的有效质量：
            // 软化有效质量=(有效质量^-1+cfm/dt)^-1

            // 这里是一些诡计发生的地方。(请注意软化和未软化有效质量之间的区别)。
            // 首先将CFM替换到软化的有效质量定义中：
            // CFm/dt=(刚度*dt+阻尼)^-1/dt=(dt*(刚度*dt+阻尼))^-1=(刚度*dt^2+阻尼*dt)^-1
            // 软化有效质量=(有效质量^-1+(刚度*dt^2+阻尼*dt)^-1)^-1
            // 现在替身给出了刚度和阻尼的定义,将标量分量视为维数等于有效质量的统一缩放矩阵：
            // 软化有效质量=(有效质量^-1+((有效质量*自然频率^2)*dt^2+(有效质量*2*阻尼比*自然频率)*dt)^-1)^-1
            // 结合内部有效质量系数,给定矩阵乘法分布在加法上：
            // 软化有效质量=(有效质量^-1+(有效质量*(自然频率^2*dt^2)+有效质量*(2*阻尼比*自然频率*dt))^-1)^-1
            // 软化有效质量=(有效质量^-1+(有效质量*(自然频率^2*dt^2+2*阻尼比*自然频率*dt))^-1)^-1
            // 应用内部矩阵求逆：
            // 软化有效质量=(有效质量^-1+(自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1*有效质量^-1)^-1
            // 再次合并内部有效质量^-1项的系数：
            // 软化有效质量=((1+(自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1)*有效质量^-1)^-1
            // 再次应用反转：
            // 软化有效质量=有效质量*(1+(自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1)^-1

            // 换言之,因为CFM是基于有效质量的,所以将其应用于有效质量会导致简单的缩尺。

            // 有什么收获？考虑求解迭代中发生的情况。
            // 我们取速度误差：
            // 速度错误=偏置-累积脉冲*cfm/dt-wsv*jt
            // 并将其转换为具有有效质量的校正脉冲：
            // 脉冲=(偏置-累积脉冲*cfm/dt-wsv*jt)*软化有效质量
            // 有效质量分布在集合上：
            // 脉冲=偏置*软化有效质量-累积脉冲*cfm/dt*软化有效质量-wsv*jt*软化有效质量
            // 关注CFM术语：
            // -累积脉冲*cfm/dt*软化有效质量
            // 什么是CFM/DT*softenedEffectiveMass？替身。
            // (刚度*dt^2+阻尼*dt)^-1*软化有效质量
            // ((有效质量*自然频率^2)*dt^2+(有效质量*2*阻尼比*自然频率*dt))^-1*软化有效质量
            // 合并术语：
            // (有效质量*(自然频率^2*dt^2+2*阻尼比*自然频率*dt))^-1*软化有效质量
            // 应用反转：
            // (自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1*有效质量^-1*软化有效质量
            // 从早期展开软化的有效体量：
            // (自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1*有效质量^-1*有效质量*(1+(自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1)^-1
            // 取消有效群众：(！)
            // (自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1*(1+(自然频率^2*dt^2+2*阻尼比*自然频率*dt)^-1)^-1
            // 因为CFM是从ffectiveMass中创建的,所以cfm/dt*ffectiveMass项实际上与ffectiveMass值无关！
            // 剩下的表达式仍然是一个矩阵,但幸运的是,它是一个简单的统一缩放矩阵,我们可以将其作为单个标量存储和应用。

            // 4)如何计算ERP？
            // ERP=(刚度*DT)*CFM
            // ERP=(刚度*dt)*(刚度*dt+阻尼)^-1
            // ERP=((有效质量*自然频率^2)*dt)*((有效质量*自然频率^2)*dt+(有效质量*2*阻尼比*自然频率))^-1
            // 组合分母术语：
            // ERP=((有效质量*自然频率^2)*dt)*((有效质量*(自然频率^2*dt+2*阻尼比*自然频率))^-1
            // 应用分母反转：
            // ERP=((有效质量*自然频率^2)*dt)*(自然频率^2*dt+2*阻尼比*自然频率)^-1*有效质量^-1
            // 均匀缩放矩阵交换：
            // ERP=(自然频率^2*dt)*有效质量*有效质量^-1*(自然频率^2*dt+2*阻尼比*自然频率)^-1
            // 取消！
            // ERP=(自然频率^2*dt)*(自然频率^2*dt+2*阻尼比*自然频率)^-1
            // ERP=(自然频率*dt)*(自然频率*dt+2*阻尼比)^-1
            // ERP是一个简单的标量,与质量无关。

            // 5)这样我们就可以计算CFM,ERP,软化的有效质量矩阵,并且在求解迭代的约束力混合项上有了一个有趣的捷径。
            // 还能做些什么吗？行!没问题!
            // 让我们再看一看分发后的脉冲计算：
            // 脉冲=偏置*有效质量-累计脉冲*cfm/dt*有效质量-wsv*jt*有效质量
            // 在解算迭代过程中,唯一变化的量是累积的脉冲和世界空间速度。这样睡觉就可以预计算了。
            // 偏移*有效质量,
            // cfm/dt*有效质量,
            // JT*有效质量
            // 换句话说,我们绕过中间速度状态,直接从震源速度转到冲量。
            // 请注意以上预计算类型的大小：
            // 偏移*有效质量与偏移大小相同(尺寸等于约束自由度的向量)
            // cfm/dt*有效质量是单个标量,与约束自由度无关,
            // JT*有效质量与JT大小相同
            // 但请注意,我们不再需要加载有效质量！它是含蓄的。
            // 由此产生的计算结果为：
            // 脉冲=a-累计脉冲*b-wsv*c
            // 两个自由度宽度相加(加/减)、一个自由度宽度相乘和一个1xDOF*DOFx12雅可比大小的变换。
            // 与…比较;
            // (偏置-累计脉冲*cfm/dt-wsv*jt)*有效质量
            // 两个自由度宽度相加(加/减)、一个自由度宽度相乘、1xDOF*DOFx12雅可比大小变换和1xDOF*DOFxDOF变换。
            // 换句话说,我们在每次迭代中剔除了完整的1xDOF*DOFxDOF变换。
            // 因此,单独来看,这在内存和计算量方面都是一次严格的胜利。

            // 不幸的是,这并不是那么简单-雅各布也被用来将冲动转换到世界空间,这样它就可以用来改变身体的速度。
            // 我们还需要把这些东西放在身边。所以当我们不再存储有效质量时,我们的雅可比被复制了。
            // 但是等等,还有更多！

            // 这个过程看起来是这样的：
            // wsv+=脉冲*J*M^-1
            // 因此,虽然我们需要在这里存储一些东西,但我们可以利用这样一个事实,即我们没有在其他任何地方使用雅可比(它被上面的JT*EfftiveMassTM项取代)。
            // 预计算J*M^-1也是如此。
            // 所以您仍然在加载一个雅可比大小的矩阵,但是您不需要加载M^-1！这为您节省了14个标量。(对称3x3+1+对称3x3+1)
            // 这就省去了(Pulse*J)*M^-1的乘法,即6个乘法和6个点积。

            // 请注意,此优化的值取决于约束自由度的数量。

            // 净内存更改,opt与no opt,以标量表示：
            // 1自由度：成本1x12,节省1x1有效质量和14标量M^-1：-3
            // 2自由度：成本2x12,节省2x2对称有效质量和14标量M^-1：7
            // 3自由度：成本3x12,节省3x3对称有效质量和14标量M^-1：16
            // 4自由度：成本4x12,节省4x4对称有效质量和14标量M^-1：24
            // 5自由度：成本5x12,节省5x5对称有效质量和14标量M^-1：31
            // 6自由度：成本6x12,节省6x6对称有效质量和14标量M^-1：37

            // 净计算节省,选择与不选择：
            // 节省自由度=1xDOF*DOFxDOF(DOF DOFdot积)、2 1x3*标量(6倍)、2 1x3*3x3(6 3点积)
            // =(DOF*DOF乘法+DOF*(DOF-1)加法)+(6个乘法)+(18个乘法+12个加法)
            // =DOF*DOF+24相乘,DOF*DOF-DOF+12相加
            // 1自由度：25乘法,12加法
            // 2自由度：28乘法,14加法
            // 3自由度：33乘法,18加法
            // 4自由度：40次乘法,24次加法
            // 5自由度：49乘法,32加法
            // 6自由度：60次乘法,42次加法

            // 那么,我们的"优化"实际上做了什么有用的事情吗？
            // 在一个自由度的限制下,这通常是一场没有任何不利因素的胜利。
            // 2+很难测定。
            // 这在很大程度上取决于机器的SIMD宽度。您并行执行每个通道的ALU操作,但是负载基本上仍然受内存带宽的限制。
            // 负载是连贯的,至少--这些东西上没有聚集。但是,如果3DOF+约束最终在宽SIMD上*没有*预转换的情况下变得更快,我也不会感到惊讶。
            // 这只是需要逐个案例分析的事情。约束可以具有特殊的结构,从而改变判断。

            // (另外,请注意大型自由度的雅克比通常非常稀疏。考虑6自由度焊接接头所使用的雅克比。您可能会进行特殊情况的优化,以减少
            // 再往上装。您不太可能找到对JT*EffectiveMasss执行相同操作的方法。不过,J*M^-1可能有一些节省。但J*M^-1并不*更稀疏*
            // 而不是J本身,所以节省的空间是有限的。只要预先计算,上述负载需求偏移量就会保持不变。)

            // 不过,这是个好消息！这个技巧适用的地方有很多限制。

            // 我们将从未软化的有效质量开始,它由上面计算的贡献构成：
            var effectiveMass = Vector<float>.One / (linearA + linearB + angularA + angularB);

            SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
            var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale;

            // 请注意,我们在计算偏置速度时使用了一些破解？？即使我们的阻尼比/自然频率意味着强烈的弹性响应。
            // 这可能会导致明显的速度超调,我们应用一个任意的钳位值来保持它的合理性。
            // 这对于各种不平等约束(如接触)非常有用,因为您并不总是希望它们表现为真正的弹簧。
            var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity);
            projection.BiasImpulse = biasVelocity * softenedEffectiveMass;

            // 预计算wsv*(JT*softenedEffectiveMass)项。
            // 请注意,我们将其存储在Vector3Wide中,就好像它是行向量一样,但这实际上是一列(因为JT是列向量)。
            // 所以我们实际上存储了(JT*softenedEffectiveMassT=softenedEffectiveMassT*J)T=softenedEffectiveMassT*J。
            // 由于此约束为1自由度,因此softenedEffectiveMass值是标量,顺序无关紧要。
            // 在求解迭代中,WSVtoCSI项将在转换期间被转置,
            // 从而产生正确的wsv*(softenedEffectiveMassT*J)T=wsv*(JT*softenedEffectiveMass.
            // 您将看到在更高自由度的约束中重复此模式。我们显式计算SoftenedEffectiveMassT*J,然后在求解中应用转置。
            // (为什么？因为创建Matrix3x2和Matrix2x3和4x3以及3x4和5x3和3x5等等只会使几乎没有价值的表示的数量增加一倍。)
            Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA);
            Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA);
            Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB);
            Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB);
        }
        // 命名约定：
        // 我们在两个空间之间进行变换,世界空间和约束空间。我们还讨论两个量--速度和脉冲。
        // 我们有一些实体参与到约束中。因此：
        // WSVA：物体A的世界空间速度
        // wsvb：物体B的世界空间速度
        // csvError：约束空间速度错误-当将物体速度投影到约束空间并结合速度偏差时,结果是单个约束速度错误
        // CSVA：约束物体A的空间速度;投影到转置(JacobianA)上的世界空间速度
        // csva线性：物体A的线速度对约束空间速度的贡献


        /// <summary>
        /// 将冲量从约束空间变换到世界空间,并使用它修改身体的缓存世界空间速度。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector<float> correctiveImpulse,
            ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            // 应用冲量需要将约束空间冲量转换为世界空间速度更改。
            // 第一步是变换为世界空间冲量,这需要通过转置的雅可比变换。
            // (转置(Jacobian)从世界空间到约束空间,Jacobian从约束空间到世界空间)。
            // 然后,通过将冲量按质量/惯量的倒数进行缩放,将该世界空间冲量转换为校正的速度变化。
            // 作为对具有较小雅可比的约束的优化,雅可比*(惯性或质量)变换被预先计算。
            BodyVelocities correctiveVelocityA, correctiveVelocityB;
            Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear);
            Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular);
            Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear);
            Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular);
            Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear);
            Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular);
            Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear);
            Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WarmStart(ref Projection2Body1DOF data, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            // TODO：如果上一帧和当前帧与不同的时间步长相关联,则上一帧的解决方案将不再是一个好的解决方案。
            // 为了补偿这一点,如果DT改变,应该对累积的脉冲进行定标。
            ApplyImpulse(ref data, ref accumulatedImpulse, ref wsvA, ref wsvB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector<float> accumulatedImpulse,
            out Vector<float> correctiveCSI)
        {
            // 通过转置(雅可比)变换,将每个物体的世界空间速度带入约束空间。
            // (按照惯例,雅可比是行向量,而出于约束空间速度计算的目的,我们将速度向量视为12x1行向量。
            // 所以我们把v*jt相乘。)
            // 然后,通过施加有效质量将其转换为冲量。
            // 这里,我们将投影和脉冲转换组合成一个预计算值,即v*(JT*softenedEffectiveMass)。
            Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear);
            Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular);
            Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear);
            Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular);
            // 将它们组合在一起,如下所示：
            // 约束空间脉冲=(目标速度-当前速度)*柔化有效质量
            // 约束空间脉冲=(偏移-累积脉冲*柔和度-wsv*jt)*软化有效质量
            // 约束空间脉冲=(偏移*软化有效质量)-累积的脉冲*(柔度*软化有效质量)-wsv*(JT*软化有效质量)
            var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);

            var previousAccumulated = accumulatedImpulse;
            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse + csi);

            correctiveCSI = accumulatedImpulse - previousAccumulated;

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(ref Projection2Body1DOF projection, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI);
            ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB);

        }

    }
}
